/***************************************************************
*
* M3D_FLOAT2INT.CPP
*
* Float-2-int conversions
***************************************************************/
#include <ap_comm.h>
#include <m3d.h>
#include <m3d/m3d_float2int_vmx.h>

#if defined _AP_XENON
   #include <ap/ap_comm_xenon.h>
#endif

#include "ds\ds_string.h"


/*************************************************************
* SPU shared code
*************************************************************/
/***************************************************************
* m3dPrepareCompressVert ()
***************************************************************/
void m3dPrepareCompressVert (const m3dBOX *pBB, ds::ARRAY<INT16, 3> & vertOffset, ds::ARRAY<INT16, 3> & vertScale)
{
   vertOffset[0] = int((m3dFloor(pBB->bll.x, 1.f) + m3dCeil(pBB->fur.x, 1.f)) / 2);
   vertOffset[1] = int((m3dFloor(pBB->bll.y, 1.f) + m3dCeil(pBB->fur.y, 1.f)) / 2);
   vertOffset[2] = int((m3dFloor(pBB->bll.z, 1.f) + m3dCeil(pBB->fur.z, 1.f)) / 2);

   int scaleX    = int(max(1.f, max(abs(m3dCeil(pBB->fur.x - vertOffset[0], 1.f)), abs(m3dFloor(pBB->bll.x - vertOffset[0], 1.f)))));
   int scaleY    = int(max(1.f, max(abs(m3dCeil(pBB->fur.y - vertOffset[1], 1.f)), abs(m3dFloor(pBB->bll.y - vertOffset[1], 1.f)))));
   int scaleZ    = int(max(1.f, max(abs(m3dCeil(pBB->fur.z - vertOffset[2], 1.f)), abs(m3dFloor(pBB->bll.z - vertOffset[2], 1.f)))));

   STRONG_ASSERT3(scaleX > 0 && scaleX < 32767, scaleX, scaleY, scaleZ);
   STRONG_ASSERT3(scaleY > 0 && scaleY < 32767, scaleX, scaleY, scaleZ);
   STRONG_ASSERT3(scaleZ > 0 && scaleZ < 32767, scaleX, scaleY, scaleZ);

   vertScale[0]  = scaleX;
   vertScale[1]  = scaleY;
   vertScale[2]  = scaleZ;
   return;
}
/***************************************************************
* m3dCompressVert ()
***************************************************************/
void m3dCompressVert (const INT16 *vertOffset, const INT16 *vertScale, m3dV * v, INT16 * res)
{
   float offset_x = (float)(vertOffset[0]);
   float offset_y = (float)(vertOffset[1]);
   float offset_z = (float)(vertOffset[2]);

   float scale_x = (float)vertScale[0];
   float scale_y = (float)vertScale[1];
   float scale_z = (float)vertScale[2];

   STRONG_ASSERT2(abs(m3dRound(v->x / scale_x * 32767.f) - m3dRound(offset_x / scale_x * 32767.f)) <= 32768.f, (v->x / scale_x * 32767.f), (offset_x / scale_x * 32767.f));
   STRONG_ASSERT2(abs(m3dRound(v->y / scale_y * 32767.f) - m3dRound(offset_y / scale_y * 32767.f)) <= 32768.f, (v->y / scale_y * 32767.f), (offset_y / scale_y * 32767.f));
   STRONG_ASSERT2(abs(m3dRound(v->z / scale_z * 32767.f) - m3dRound(offset_z / scale_z * 32767.f)) <= 32768.f, (v->z / scale_z * 32767.f), (offset_z / scale_z * 32767.f));

   float res_x = m3dRound(v->x / scale_x * 32767.f) - m3dRound(offset_x / scale_x * 32767.f);
   float res_y = m3dRound(v->y / scale_y * 32767.f) - m3dRound(offset_y / scale_y * 32767.f);
   float res_z = m3dRound(v->z / scale_z * 32767.f) - m3dRound(offset_z / scale_z * 32767.f);

   res[0] = (INT16)_m3dClamp(-32767.f, 32767.f, res_x);
   res[1] = (INT16)_m3dClamp(-32767.f, 32767.f, res_y);
   res[2] = (INT16)_m3dClamp(-32767.f, 32767.f, res_z);
   return;
}

/***************************************************************
* m3dCompressVert_4 ()
***************************************************************/
void m3dCompressVert_4 (const INT16 *vertOffset, const INT16 *vertScale, m3dV *v, INT16 *_v)
{
//   m3dCompressVert(vertOffset, vertScale, v, _v);
//   _v[3] = 32767; // 1.0f

   _v[0] = (INT16)(_m3dClamp(-1.f, 1.f, (v->x - vertOffset[0]) / vertScale[0]) * 32767);
   _v[1] = (INT16)(_m3dClamp(-1.f, 1.f, (v->y - vertOffset[1]) / vertScale[1]) * 32767);
   _v[2] = (INT16)(_m3dClamp(-1.f, 1.f, (v->z - vertOffset[2]) / vertScale[2]) * 32767);
   _v[3] = 32767; // 1.0f
   return;
}

/***************************************************************
* m3dDecompressVert ()
***************************************************************/
void m3dDecompressVert (const INT16 *vertOffset, const INT16 *vertScale, INT16 *v, m3dV *_v)
{
   vmxV offset, scale;
   m3dPrepareDecompressVert(vertOffset, vertScale, offset, scale);
   m3dDecompressVert(offset, scale, v, _v);
   return;
}

/***************************************************************
* m3dCompressNormal_FP32 ()
***************************************************************/
void m3dCompressNormal_FP32 (const m3dV *pnorm, float *_norm)
{
   ASSERT(m3dLengthVector(pnorm) > 0.1f && m3dLengthVector(pnorm) < 1.5f);

   m3dV norm = *pnorm;
/*
   norm.x = fpmin(norm.x, 1.f);
   norm.y = fpmin(norm.y, 1.f);
   norm.z = fpmin(norm.z, 1.f);

   norm.x = fpmax(norm.x, -1.f);
   norm.y = fpmax(norm.y, -1.f);
   norm.z = fpmax(norm.z, -1.f);
*/
   norm.x = 255.5f*(0.5f*norm.x+0.5f);
   norm.y = 255.5f*(0.5f*norm.y+0.5f);
   norm.z = 255.5f*(0.5f*norm.z+0.5f);

   norm.x = (float)fpmax(fpmin(norm.x, 255.5f), 0.f);
   norm.y = (float)fpmax(fpmin(norm.y, 255.5f), 0.f);
   norm.z = (float)fpmax(fpmin(norm.z, 255.5f), 0.f);

   *_norm = m3dIntFloor(norm.x)+256.f*m3dIntFloor(norm.y)+256.f*256.f*m3dIntFloor(norm.z);

#ifdef _DEBUG
   m3dV newNorm;
   m3dDecompressNormal_FP32(_norm, &newNorm);
   ASSERT(m3dAngleVector_COS(pnorm, &newNorm) > 0.98f);
   ASSERT(m3dLengthVector(&newNorm) > 0.1f);
/*
   m4dV oldNorm __attribute__((aligned (16)));
   oldNorm = *pnorm;
   float __norm;
   m3dCompressNormal_FP32_VMX_4A(&oldNorm, &__norm);
   ASSERT(m3dIsEqual(*_norm, __norm));
*/
#endif
   return;
}

/***************************************************************
* m3dDecompressNormal_FP32 ()
***************************************************************/
void m3dDecompressNormal_FP32(const float *norm, m3dV  *_norm)
{
   float tmp;
   _norm->x = -1.f+2.f*modf(*norm / 256.f, &tmp);
   _norm->y = -1.f+2.f*modf(*norm / 256.f / 256.f, &tmp);
   _norm->z = -1.f+2.f*modf(*norm / 256.f / 256.f / 256.f, &tmp);
   return;
}
/***************************************************************
* m3dCompressNormal_8_8 ()
***************************************************************/
void m3dCompressNormal_8_8 (const m3dV * pnorm, INT16 *_norm)
{
   m3dV norm = *pnorm;
   _m3dNormalize(&norm);

   int _x = m3dIntRound(0.51f+179.49f*(0.5f*norm.x+0.5f));
   int _z = m3dIntRound(180.f*(0.5f*norm.z+0.5f));

   INT16 n;
   int best_x = -1, best_z = -1;
   float maxAngle = M3D_INFINITY;
   for (int x = _x-2; x <= _x+2; x ++) {
      if (x < 1 || x > 180) {
         continue;
      }
      for (int z = _z-2; z <= _z+2; z ++) {
         if (z < 0 || z > 180) {
            continue;
         }
         n = x+z*181;
         if (norm.y < 0) {
            n = -n;
         }
         
         m3dV newNorm;
         m3dDecompressNormal_8_8(&n, &newNorm);
         float dot = m3dFAbs(1.f-m3dDotProduct(&norm, &newNorm));
         if (dot < maxAngle) {
            best_x   = x;
            best_z   = z;
            maxAngle = dot;
         }
      }
   }

   *_norm = best_x+best_z*181;
   if (norm.y < 0) {
      *_norm = -(*_norm);
   }

#ifdef _DEBUG
   m3dV newNorm;
   m3dDecompressNormal_8_8(_norm, &newNorm);
   ASSERT(m3dAngleVector_COS(&norm, &newNorm) > 0.96f);
   ASSERT(m3dLengthVector(&newNorm) > 0.96f);

/*
   m4dV newNorm1 __attribute__((aligned (16)));
   m3dDecompressNormal_8_8_VMX_4A(*_norm, &newNorm1);
   ASSERT(m3dDotProduct(&norm, &newNorm1) > 0.96f);
*/
#endif

   return;
}
/***************************************************************
* m3dDecompressNormal_8_8 ()
***************************************************************/
void m3dDecompressNormal_8_8 (const INT16 *norm, m3dV  *_norm)
{
   float tmp;
   _norm->x = -1.f+2.f*modf(m3dFAbs(*norm) / 181.f, &tmp);
   _norm->z = -1.f+2.f*modf(m3dFAbs(*norm) / 181.f / 181.f, &tmp);
                       
   tmp = m3dPow2(_norm->x) + m3dPow2(_norm->z);
   tmp = m3dFMin(tmp, 1.f);
   ASSERT(tmp <= 1.f);
   _norm->y = m3dSqrt(max(0.f, 1.f - tmp));
   if (*norm < 0) {
      _norm->y = -_norm->y;
   }
   return;
}


/*************************************************************
* End of SPU shared code
*************************************************************/





#ifndef SPU

struct m3dCONV_PREP {
   m3dCONV_POS48  convPos48;
   m3dCONV        convVectLen16;
   m3dCONV        convPhi8;
   m3dCONV        convTheta8;
   m3dCONV        convPhi12;
   m3dCONV        convTheta12;

   m3dCONV_PREP(void);
};

static m3dCONV_PREP  _m3dConvPrep;

/***************************************************************
* m3dCONV_FAST::Init ()
*
***************************************************************/
void m3dCONV_FAST::Init (int _nBit, float _a, float _b)
{
   a  = _a;
   b  = _b;
   fa = 0;
   fb = (2 << (_nBit-1)) - 1;

   c1_Int = fa - a * (fb - fa) / (b - a);
   c2_Int =          (fb - fa) / (b - a);

   c1_Flt = a - fa * (b - a) / (fb - fa);
   c2_Flt =          (b - a) / (fb - fa);

   return;
}

/***************************************************************
* m3dCONV_TIGHT::Init()
*
***************************************************************/
void m3dCONV_TIGHT::Init (int _nBit, float _a, float _b)
{
   DWORD  dw;

   nBit = _nBit;

   dw = m3dConvFloatToNBit(_a, -5000.f, 5000.f, 16);
   ASSERT((dw & 0xFFFF0000) == 0);
   aShort = (WORD)(dw & 0x0000FFFF);

   dw = m3dConvFloatToNBit(_b, -5000.f, 5000.f, 16);
   ASSERT((dw & 0xFFFF0000) == 0);
   bShort = (WORD)(dw & 0x0000FFFF);
   return;
}

/***************************************************************
* m3dCONV_TIGHT::ConvInt()
*
***************************************************************/
DWORD m3dCONV_TIGHT::ConvInt (float t)
{
   float a, b;
   DWORD dw;

   a = m3dConvNBitToFloat(aShort, 16, -5000.f, 5000.f);
   b = m3dConvNBitToFloat(bShort, 16, -5000.f, 5000.f);

   dw = m3dConvFloatToNBit(t, a, b, nBit);
   return dw;
}

/***************************************************************
* m3dCONV_TIGHT::ConvFlt()
*
***************************************************************/
float m3dCONV_TIGHT::ConvFlt (int t)
{
   float a, b, res;

   a = m3dConvNBitToFloat(aShort, 16, -5000.f, 5000.f);
   b = m3dConvNBitToFloat(bShort, 16, -5000.f, 5000.f);

   res = m3dConvNBitToFloat(t, nBit, a, b);
   return res;
}

/***************************************************************
* m3dCONV_PREP::m3dCONV_PREP ()
*
***************************************************************/
m3dCONV_PREP::m3dCONV_PREP (void) 
{
   convVectLen16.Init(16, 0.f, 500.f);

   convPhi8.Init(8, 0.f, 180.f);
   convTheta8.Init(8, -180.f, 180.f);

   convPhi12.Init(16, 0.f, 180.f);
   convTheta12.Init(16, -180.f, 180.f);
   return;
}

/***************************************************************
* m3dConvFloatToN()
*
***************************************************************/
DWORD m3dConvFloatToNBit (float val, float rangeMin, float rangeMax, int nBit)
{
   DWORD    res;

   ASSERT(val >= rangeMin-M3D_EPSILON && val <= rangeMax+M3D_EPSILON);
   res = (DWORD)m3dClamp(0.f, (float)((2 << (nBit-1)) - 1), rangeMin, rangeMax, val);
   return res;
}

/***************************************************************
* m3dConvNBitToFloat()
*
***************************************************************/
float m3dConvNBitToFloat (DWORD val, int nBit, float rangeMin, float rangeMax)
{
   float    res;

   ASSERT(val < ((2 << nBit) - 1));
   res = m3dClamp(rangeMin, rangeMax, 0.f, (float)((2 << (nBit-1)) - 1), (float)val);
   ASSERT(res >= rangeMin && res <= rangeMax);
   return res;
}

/***************************************************************
* m3dConvFloatTo8()
*
***************************************************************/
UCHAR m3dConvFloatTo8 (float val, float rangeMin, float rangeMax)
{
   UCHAR    res;

   ASSERT(val >= rangeMin && val <= rangeMax);
   res = (UCHAR)m3dClamp(0.f, 255.f, rangeMin, rangeMax, val);
   return res;
}

/***************************************************************
* m3dConv8ToFloat()
*
***************************************************************/
float m3dConv8ToFloat (UCHAR val, float rangeMin, float rangeMax)
{
   float    res;

   res = m3dClamp(rangeMin, rangeMax, 0.f, 255.f, (float)val);
   ASSERT(res >= rangeMin && res <= rangeMax + 0.001f);
   return res;
}

/***************************************************************
* m3dConvFloatTo16()
*
***************************************************************/
USHORT m3dConvFloatTo16 (float val, float rangeMin, float rangeMax)
{
   USHORT    res;

   ASSERT(val >= rangeMin && val <= rangeMax);
   res = (USHORT)m3dClamp(0.f, 65535.f, rangeMin, rangeMax, val);
   return res;
}

/***************************************************************
* m3dConv16ToFloat()
*
***************************************************************/
float m3dConv16ToFloat (USHORT val, float rangeMin, float rangeMax)
{
   float    res;

   res = m3dClamp(rangeMin, rangeMax, 0.f, 65535.f, (float)val);
   ASSERT(res >= rangeMin && res <= rangeMax);
   return res;
}


/***************************************************************
* m3dConvDirTo16 ()
*
***************************************************************/
void m3dConvDirTo16 (m3dV *dir, UCHAR *phiC, UCHAR *thetaC)
{
   float    phi, theta;

   ASSERT(m3dIsZero_A(m3dLengthVector(dir) - 1.f, 0.01f));

   phi   = m3dAngleVector(dir, &m3dVUnitY);
   theta = m3xzAngleVector(dir, &m3dVUnitZ);

   *phiC =   (UCHAR)_m3dConvPrep.convPhi8.ConvInt(phi);
   *thetaC = (UCHAR)_m3dConvPrep.convTheta8.ConvInt(theta);

//   *phiC = m3dConvFloatTo8(phi, 0.f, 180.f);
//   *thetaC = m3dConvFloatTo8(theta, -180.f, 180.f);

   return;
}
/***************************************************************
* m3dConv16ToDir()
*
***************************************************************/
void m3dConv16ToDir (UCHAR phiC, UCHAR thetaC, m3dV *dir)
{
   float    phi, theta;
   m3dMATR  m;

   phi   = _m3dConvPrep.convPhi8.ConvFlt(phiC);
   theta = _m3dConvPrep.convTheta8.ConvFlt(thetaC);

//   phi = m3dConv8ToFloat(phiC, 0.f, 180.f);
//   theta = m3dConv8ToFloat(thetaC, -180.f, 180.f);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, dir);

   m.MakeRotY(-theta);
   m.TransformVector(dir);

   return;
}

/***************************************************************
* m3dConvDirTo24()
*
***************************************************************/
void m3dConvDirTo24 (m3dV *dir, UCHAR *b1, UCHAR *b2, UCHAR *b3)
{
   float    phi, theta;
   DWORD    d1, d2;

   ASSERT(m3dIsZero_A(m3dLengthVector(dir) - 1.f, 0.01f));

   phi   = m3dAngleVector(dir, &m3dVUnitY);
   theta = m3xzAngleVector(dir, &m3dVUnitZ);

   d1 = _m3dConvPrep.convPhi12.ConvInt(phi);
   d2 = _m3dConvPrep.convTheta12.ConvInt(theta);
//   d1 = m3dConvFloatToNBit(phi, 0.f, 180.f, 12);
//   d2 = m3dConvFloatToNBit(theta, -180.f, 180.f, 12);

   *b1 = (UCHAR)(d1 & 0x000000FF);
   *b2 = (UCHAR)(d2 & 0x000000FF);
   *b3 = (UCHAR)(((d1 & 0x00000F00) >> 8) | ((d2 & 0x00000F00) >> 4));

   return;
}

/***************************************************************
* m3dConv24ToDir()
*
***************************************************************/
void m3dConv24ToDir (UCHAR b1, UCHAR b2, UCHAR b3, m3dV *dir)
{
   float    phi, theta;
   m3dMATR  m;
   DWORD    d1, d2;

   d1 = b1 | (((DWORD)b3 & 0x0000000F) << 8);
   d2 = b2 | (((DWORD)b3 & 0x000000F0) << 4);

   phi   = _m3dConvPrep.convPhi12.ConvFlt(d1);
   theta = _m3dConvPrep.convTheta12.ConvFlt(d2);
//   phi   = m3dConvNBitToFloat(d1, 12, 0.f, 180.f);
//   theta = m3dConvNBitToFloat(d2, 12, -180.f, 180.f);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, dir);

   m.MakeRotY(-theta);
   m.TransformVector(dir);

   return;
}

/***************************************************************
* m3dConvVectorTo32 ()
*
***************************************************************/
void m3dConvVectorTo32 (m3dV *speed, UCHAR *phiC, UCHAR *thetaC, USHORT *lenS)
{
   float    phi, theta, len;

   phi   = m3dAngleVector(speed, &m3dVUnitY);
   theta = m3xzAngleVector(speed, &m3dVUnitZ);
   len   = m3dLengthVector(speed);

   *phiC   = (UCHAR)_m3dConvPrep.convPhi8.ConvInt(phi);
   *thetaC = (UCHAR)_m3dConvPrep.convTheta8.ConvInt(theta);
   *lenS   = (WORD)_m3dConvPrep.convVectLen16.ConvInt(len);
//   *phiC = m3dConvFloatTo8(phi, 0.f, 180.f);
//   *thetaC = m3dConvFloatTo8(theta, -180.f, 180.f);
//   *lenS = m3dConvFloatTo16(len, 0.f, _m3dConvRange.maxVectLen);

   return;
}

/***************************************************************
* m3dConv32ToVector ()
*
***************************************************************/
void m3dConv32ToVector (UCHAR phiC, UCHAR thetaC, USHORT lenS, m3dV *speed)
{
   float    phi, theta, len;
   m3dMATR  m;

   phi   = _m3dConvPrep.convPhi8.ConvFlt(phiC);
   theta = _m3dConvPrep.convTheta8.ConvFlt(thetaC);
   len   = _m3dConvPrep.convVectLen16.ConvFlt(lenS);

//   phi = m3dConv8ToFloat(phiC, 0.f, 180.f);
//   theta = m3dConv8ToFloat(thetaC, -180.f, 180.f);
//   len = m3dConv16ToFloat(lenS, 0.f, _m3dConvRange.maxVectLen);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, speed);

   m.MakeRotY(-theta);
   m.TransformVector(speed);

   _m3dScaleVector(speed, len);

   return;
}

/***************************************************************
* m3dConvPointTo48 ()
*
* Ranges can be specified on per-level basis
***************************************************************/
void m3dConvPointTo48 (m3dV *pos, WORD *w1, WORD *w2, WORD *w3)
{
   _m3dConvPrep.convPos48.ConvPosTo48(pos, w1, w2, w3);
   return;
}

/***************************************************************
* m3dConv48ToM3DV ()
*
* Ranges can be specified on per-level basis
***************************************************************/
void m3dConv48ToPoint (WORD w1, WORD w2, WORD w3, m3dV *pos)
{
   _m3dConvPrep.convPos48.Conv48ToPos(w1, w2, w3, pos);

   return;
}



/***************************************************************
* m3dCompressTexCoord ()
***************************************************************/
void m3dCompressTexCoord (WORD maxTile, m3dVTX *v, INT16 *_v)
{
   ASSERT(v->s > -maxTile-.01f && v->s < maxTile+.01f);
   ASSERT(v->t > -maxTile-.01f && v->t < maxTile+.01f);
   _v[0] = (INT16)(_m3dClamp(-1.f, 1.f, v->s / maxTile) * 32767);
   _v[1] = (INT16)(_m3dClamp(-1.f, 1.f, v->t / maxTile) * 32767);
   return;
}
/***************************************************************
* m3dDecompressTexCoord ()
***************************************************************/
void m3dDecompressTexCoord (WORD maxTile, volatile INT16 *v, m3dVTX *_v)
{
   _v->s = float(v[0]) / 32767.f * maxTile;
   _v->t = float(v[1]) / 32767.f * maxTile;
   return;
}
/***************************************************************
* m3dCompressTexCoord_INT8 ()
***************************************************************/
void m3dCompressTexCoord (WORD maxTile, m3dVTX *v, INT8 *_v)
{
   ASSERT(v->s > -maxTile-.001f && v->s < maxTile+.001f);
   ASSERT(v->t > -maxTile-.001f && v->t < maxTile+.001f);
   _v[0] = (INT8)(_m3dClamp(-1.f, 1.f, v->s / maxTile) * 127);
   _v[1] = (INT8)(_m3dClamp(-1.f, 1.f, v->t / maxTile) * 127);
   return;
}
/***************************************************************
* m3dDecompressTexCoord ()
***************************************************************/
void m3dDecompressTexCoord (WORD maxTile, INT8 *v, m3dVTX *_v)
{
   _v->s = float(v[0]) / 127.f * maxTile;
   _v->t = float(v[1]) / 127.f * maxTile;
   return;
}

/***************************************************************
* m3dCompressNormal_11_11_10 ()
***************************************************************/
void m3dCompressNormal_11_11_10 (const m3dV *norm, DWORD *_norm)
{
   *_norm = ( ( ( (DWORD)((1 + norm->z) *  511.0f / 2 + 0.5f) ) & 0x3ff ) << 22L ) |
      ( ( ( (DWORD)((1 + norm->y) * 1023.0f / 2 + 0.5f) ) & 0x7ff ) << 11L ) |
      ( ( ( (DWORD)((1 + norm->x) * 1023.0f / 2 + 0.5f) ) & 0x7ff ) <<  0L );
   return;
}
/***************************************************************
* m3dDecompressNormal_11_11_10 ()
***************************************************************/
void m3dDecompressNormal_11_11_10 (const DWORD *norm, m3dV *_norm)
{
   int nmb; 

   nmb = ((*norm) & 0x7ff);
   _norm->x = (2 * nmb - 1023) / 1023.f;

   nmb = (((*norm) >> 11L) & 0x7ff);
   _norm->y = (2 * nmb - 1023) / 1023.f;

   nmb = (((*norm) >> 22L) & 0x3ff);
   _norm->z = (2 * nmb - 511) / 511.f;
   return;
}
/***************************************************************
* m3dCompressNormal_10_10_10 ()
***************************************************************/
void m3dCompressNormal_10_10_10 (const m3dV *norm, DWORD *_norm)
{
   *_norm = ( ( ( (DWORD)(norm->z * 511.0f) ) & 0x3ff ) << 20L ) |
            ( ( ( (DWORD)(norm->y * 511.0f) ) & 0x3ff ) << 10L ) |
            ( ( ( (DWORD)(norm->x * 511.0f) ) & 0x3ff ) <<  0L );

   return;
}
/***************************************************************
* m3dDecompressNormal_10_10_10 ()
***************************************************************/
void m3dDecompressNormal_10_10_10 (const DWORD * __RESTRICT norm, m3dV * __RESTRICT _norm)
{
	// The top version of this function is equivalent to the bottom version,
	// although it may not be obvious.  The top version is 2x faster than the
	// bottom version, as it avoids all branching and many load-hit-store
	// penalties.  This function gets called thousands of times a frame, so
	// this obfuscation is worth it.
	{
		int masks[3];
		DWORD nmbx = ((*norm) & 0x3ff);
		DWORD nmbmaskedx = nmbx & 0x1ff;
		masks[0] = nmbmaskedx;
		int subx = nmbx & 0x200 ? 1 : 0;

		DWORD nmby = ((*norm >> 10L) & 0x3ff);
		DWORD nmbmaskedy = nmby & 0x1ff;
		masks[1] = nmbmaskedy;
		int suby = nmby & 0x200 ? 1 : 0;

		DWORD nmbz = ((*norm >> 20L) & 0x3ff);
		DWORD nmbmaskedz = nmbz & 0x1ff;
		masks[2] = nmbmaskedz;
		int subz = nmbz & 0x200 ? 1 : 0;

		_norm->x = (masks[0] / 511.0f) - subx;
		_norm->y = (masks[1] / 511.0f) - suby;
		_norm->z = (masks[2] / 511.0f) - subz;
	}
	/*
	{
		static DWORD n;
		n = *norm;

		DWORD nmb;
		static m3dV test;

		nmb = ((*norm) & 0x3ff);
		if (nmb & 0x200) {
		  test.x = ((nmb & 0x1ff) / 511.f)-1;
		} else {
		  test.x =  (nmb / 511.f);
		}

		nmb = (((*norm) >> 10L) & 0x3ff);
		if (nmb & 0x200) {
		  test.y = ((nmb & 0x1ff) / 511.f)-1;
		} else {
		  test.y =  (nmb / 511.f);
		}

		nmb = (((*norm) >> 20L) & 0x3ff);
		if (nmb & 0x200) {
		  test.z = ((nmb & 0x1ff) / 511.f)-1;
		} else {
		  test.z =  (nmb / 511.f);
		}

		if (test.x != _norm->x ||
			test.y != _norm->y ||
			test.z != _norm->z)
		{
			int *foo = 0;
			*foo = 0xdeadbeef;
		}
	}
	*/

	return;
}


/***************************************************************
* m3dCompressNormal ()
***************************************************************/
void m3dCompressNormal (const m3dV *norm, BYTE *_norm)
{
   _norm[0] = (BYTE)((norm->x + 1.f) / 2.f * 255);
   _norm[1] = (BYTE)((norm->y + 1.f) / 2.f * 255);
   _norm[2] = (BYTE)((norm->z + 1.f) / 2.f * 255);
   _norm[3] = 128;
   return;
}

/***************************************************************
* m3dDecompressNormal ()
***************************************************************/
void m3dDecompressNormal (const BYTE *norm, m3dV *_norm)
{
   const BYTE *data_ptr = NULL;

#ifdef __SNC__
   BYTE arr[3];
   for (int i = 0; i < 3; i++) {
      arr[i] = norm[i];
   }
   data_ptr = arr;
#else
   data_ptr = norm;
#endif

   _norm->x = (float)data_ptr[0] * 2.f / 255.f - 1.f;
   _norm->y = (float)data_ptr[1] * 2.f / 255.f - 1.f;
   _norm->z = (float)data_ptr[2] * 2.f / 255.f - 1.f;

   return;
}

/***************************************************************
* m3dCompressTangent_16_16_16_16 ()
***************************************************************/
void m3dCompressTangent (const m4dV *tang, INT16 *_tang)
{
   _tang[0] = (INT16)(tang->x * 32767);
   _tang[1] = (INT16)(tang->y * 32767);
   _tang[2] = (INT16)(tang->z * 32767);
   _tang[3] = (INT16)(tang->w * 32767);
   return;
}
/***************************************************************
* m3dDecompressTangent ()
***************************************************************/
void m3dDecompressTangent (const INT16 *tang, m4dV *_tang)
{
   _tang->x = (float)tang[0] / 32767.f;
   _tang->y = (float)tang[1] / 32767.f;
   _tang->z = (float)tang[2] / 32767.f;
   _tang->w = (float)tang[3] / 32767.f;
   return;
}
/***************************************************************
* m3dCompressTangent ()
***************************************************************/
void m3dCompressTangent (const m4dV *tang, BYTE *_tang)
{
   _tang[0] = (BYTE)((tang->x + 1.f) / 2.f * 255);
   _tang[1] = (BYTE)((tang->y + 1.f) / 2.f * 255);
   _tang[2] = (BYTE)((tang->z + 1.f) / 2.f * 255);
   _tang[3] = (BYTE)((tang->w + 1.f) / 2.f * 255);
   return;
}
/***************************************************************
* m3dDecompressTangent ()
***************************************************************/
void m3dDecompressTangent (const BYTE * __RESTRICT tang, m4dV * __RESTRICT _tang)
{
#ifdef _AP_XENON
   __dcbt( 0, _tang);
   __dcbt(32, _tang);

   XMVECTOR tangIn = XMLoadUByte4((const XMUBYTE4*)tang);
   XMVECTOR v1     = XMVectorReplicate(2.f / 255.f);
   XMVECTOR v2     = XMVectorReplicate(-1.f);
   XMVECTOR res    = XMVectorMultiplyAdd(tangIn, v1, v2);
   XMStoreVector4(_tang, res);
#else

   const BYTE *data_ptr = NULL;
   #ifdef __SNC__
      BYTE tmp[4];
      for (int i = 0; i < 4; i++) {
         tmp[i] = tang[i];
      }
      data_ptr = tmp;
   #else
      data_ptr = tang;   
   #endif

   _tang->x = (float)data_ptr[0] * 2.f / 255.f - 1.f;
   _tang->y = (float)data_ptr[1] * 2.f / 255.f - 1.f;
   _tang->z = (float)data_ptr[2] * 2.f / 255.f - 1.f;
   _tang->w = (float)data_ptr[3] * 2.f / 255.f - 1.f;
#endif
   return;
}

/***************************************************************
* m3dConvColor888To565()
*
***************************************************************/
WORD m3dConvColor888To565 (DWORD colorIn)
{
   DWORD r, g, b;
   WORD  colorOut;

   r = (colorIn & 0x00FF0000) >> 16;
   g = (colorIn & 0x0000FF00) >> 8;
   b = (colorIn & 0x000000FF) >> 0;

   r = r >> 3;
   g = g >> 2;
   b = b >> 3;

   colorOut = (WORD)((r << 11) + (g << 5) + b);
   return colorOut;
}

/***************************************************************
* m3dConvColor565To888()
*
***************************************************************/
DWORD m3dConvColor565To888 (WORD colorIn)
{
   DWORD r, g, b;
   DWORD colorOut;

   r = (colorIn & 0xF800) >> 11;
   g = (colorIn & 0x07E0) >> 5;
   b = (colorIn & 0x001F) >> 0;

   r = r << 3;
   g = g << 2;
   b = b << 3;

   colorOut = (r << 16) + (g << 8) + b;
   return colorOut;
}

/***************************************************************
* m3dConvColor888To332()
*
***************************************************************/
UCHAR m3dConvColor888To332 (DWORD colorIn)
{
   DWORD r, g, b;
   UCHAR colorOut;

   r = (colorIn & 0x00FF0000) >> 16;
   g = (colorIn & 0x0000FF00) >> 8;
   b = (colorIn & 0x000000FF) >> 0;

   r = r >> 5;
   g = g >> 5;
   b = b >> 6;

   colorOut = (UCHAR)((r << 5) + (g << 2) + b);
   return colorOut;
}

/***************************************************************
* m3dConvColor332To888()
*
***************************************************************/
DWORD m3dConvColor332To888 (UCHAR colorIn)
{
   DWORD r, g, b;
   DWORD colorOut;

   r = (colorIn & 0xE0) >> 5;
   g = (colorIn & 0x1C) >> 2;
   b = (colorIn & 0x03) >> 0;

   r = r << 5;
   g = g << 5;
   b = b << 6;

   colorOut = (r << 16) + (g << 8) + b;
   return colorOut;
}

/***************************************************************
*
* Test function to check the correctness of float<>int converters
*
***************************************************************
void CheckConverter (void)
{
   m3dCONV_POS32 conv;
   m3dBOX        bb;
   m3dV          p, v;
   DWORD         dw;

   bb.bll.x = -10.f;
   bb.bll.y =  -5.f;
   bb.bll.z = -10.f;

   bb.fur.x =  10.f;
   bb.fur.y =   5.f;
   bb.fur.z =  10.f;

   p.x = 0.f;
   p.y = 0.f;
   p.z = 0.f;

   conv.Init(&bb);

   conv.ConvPosTo32(&p, &dw);
   conv.Conv32ToPos(dw, &v);

   return;
}
*/


/***************************************************************
* float <-> half conversions
*
* from: http://www.ogre3d.org/docs/api/html/OgreBitwise_8h-source.html
***************************************************************/

/***************************************************************
* float to half
***************************************************************/
unsigned short floatToHalfI(unsigned i)
{
   register int s =  (i >> 16) & 0x00008000;
   register int e = ((i >> 23) & 0x000000ff) - (127 - 15);
   register int m =   i        & 0x007fffff;

   if (e <= 0)
   {
      if (e < -10)
      {
         return 0;
      }
      m = (m | 0x00800000) >> (1 - e);

      return s | (m >> 13);
   }
   else if (e == 0xff - (127 - 15))
   {
      if (m == 0) // Inf
      {
         return s | 0x7c00;
      } 
      else    // NAN
      {
         m >>= 13;
         return s | 0x7c00 | m | (m == 0);
      }
   }
   else
   {
      if (e > 30) // Overflow
      {
         return s | 0x7c00;
      }

      return s | (e << 10) | (m >> 13);
   }
}

/***************************************************************
* half to float
***************************************************************/
unsigned halfToFloatI(unsigned short y)
{
   register int s = (y >> 15) & 0x00000001;
   register int e = (y >> 10) & 0x0000001f;
   register int m =  y        & 0x000003ff;

   if (e == 0)
   {
      if (m == 0) // Plus or minus zero
      {
         return s << 31;
      }
      else // Denormalized number -- renormalize it
      {
         while (!(m & 0x00000400))
         {
            m <<= 1;
            e -=  1;
         }

         e += 1;
         m &= ~0x00000400;
      }
   }
   else if (e == 31)
   {
      if (m == 0) // Inf
      {
         return (s << 31) | 0x7f800000;
      }
      else // NaN
      {
         return (s << 31) | 0x7f800000 | (m << 13);
      }
   }

   e = e + (127 - 15);
   m = m << 13;

   return (s << 31) | (e << 23) | m;
}

/***************************************************************
* mpdrConvDirToInt()
*
***************************************************************/
void mpdrConvDirToInt (m3dV *dir, UCHAR *phiC, UCHAR *thetaC)
{
   float    phi, theta;

   ASSERT(m3dIsZero_A(m3dLengthVector(dir) - 1.f, 0.01f));

   phi   = m3dAngleVector(dir, &m3dVUnitY);
   theta = m3xzAngleVector(dir, &m3dVUnitZ);

   *phiC = m3dConvFloatTo8(phi, 0.f, 180.f);
   *thetaC = m3dConvFloatTo8(theta, -180.f, 180.f);

   return;
}

/***************************************************************
* mpdrConvIntToDir()
*
***************************************************************/
void mpdrConvIntToDir (UCHAR phiC, UCHAR thetaC, m3dV *dir)
{
   float    phi, theta;
   m3dMATR  m;

   phi = m3dConv8ToFloat(phiC, 0.f, 180.f);
   theta = m3dConv8ToFloat(thetaC, -180.f, 180.f);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, dir);

   m.MakeRotY(-theta);
   m.TransformVector(dir);

   return;
}

/***************************************************************
* mpdrConvDirTo3Byte()
*
***************************************************************/
void mpdrConvDirTo3Byte (m3dV *dir, UCHAR *b1, UCHAR *b2, UCHAR *b3)
{
   float    phi, theta;
   DWORD    d1, d2;

   ASSERT(m3dIsZero_A(m3dLengthVector(dir) - 1.f, 0.01f));

   phi   = m3dAngleVector(dir, &m3dVUnitY);
   theta = m3xzAngleVector(dir, &m3dVUnitZ);

   d1 = m3dConvFloatToNBit(phi, 0.f, 180.f, 12);
   d2 = m3dConvFloatToNBit(theta, -180.f, 180.f, 12);

   *b1 = (UCHAR)(d1 & 0x000000FF);
   *b2 = (UCHAR)(d2 & 0x000000FF);
   *b3 = (UCHAR)(((d1 & 0x00000F00) >> 8) | ((d2 & 0x00000F00) >> 4));

   return;
}

/***************************************************************
* mpdrConv3ByteToDir()
*
***************************************************************/
void mpdrConv3ByteToDir (UCHAR b1, UCHAR b2, UCHAR b3, m3dV *dir)
{
   float    phi, theta;
   m3dMATR  m;
   DWORD    d1, d2;

   d1 = b1 | (((DWORD)b3 & 0x0000000F) << 8);
   d2 = b2 | (((DWORD)b3 & 0x000000F0) << 4);

   phi   = m3dConvNBitToFloat(d1, 12, 0.f, 180.f);
   theta = m3dConvNBitToFloat(d2, 12, -180.f, 180.f);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, dir);

   m.MakeRotY(-theta);
   m.TransformVector(dir);

   return;
}

/***************************************************************
* mpdrConvM3DVToAngLen()
*
***************************************************************/
void mpdrConvM3DVToIntAngLen (m3dV *speed, UCHAR *phiC, UCHAR *thetaC, USHORT *lenS)
{
   float    phi, theta, len;

   phi   = m3dAngleVector(speed, &m3dVUnitY);
   theta = m3xzAngleVector(speed, &m3dVUnitZ);
   len   = m3dLengthVector(speed);

   *phiC = m3dConvFloatTo8(phi, 0.f, 180.f);
   *thetaC = m3dConvFloatTo8(theta, -180.f, 180.f);
   *lenS = m3dConvFloatTo16(len, 0.f, 500.f);

   return;
}

/***************************************************************
* mpdrConvIntAngLenToM3DV()
*
***************************************************************/
void mpdrConvIntAngLenToM3DV (UCHAR phiC, UCHAR thetaC, USHORT lenS, m3dV *speed)
{
   float    phi, theta, len;
   m3dMATR  m;

   phi = m3dConv8ToFloat(phiC, 0.f, 180.f);
   theta = m3dConv8ToFloat(thetaC, -180.f, 180.f);
   len = m3dConv16ToFloat(lenS, 0.f, 500.f);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, speed);

   m.MakeRotY(-theta);
   m.TransformVector(speed);

   _m3dScaleVector(speed, len);

   return;
}

/***************************************************************
* mpdrConvIntAngLenToM3DVHiRes()
*
***************************************************************/
void mpdrConvIntAngLenToM3DVHiRes (USHORT phiC, USHORT thetaC, USHORT lenS, m3dV *speed)
{
   float    phi, theta, len;
   m3dMATR  m;

   phi = m3dConv16ToFloat(phiC, 0.f, 180.f);
   theta = m3dConv16ToFloat(thetaC, -180.f, 180.f);
   len = m3dConv16ToFloat(lenS, 0.f, 500.f);

   // reconstruct 'speed' vector
   m.MakeRotX(phi);
   m.TransformVector(&m3dVUnitY, speed);

   m.MakeRotY(-theta);
   m.TransformVector(speed);

   _m3dScaleVector(speed, len);

   return;
}

/***************************************************************
* mpdrConvM3DVToAngLenHiRes()
*
***************************************************************/
void mpdrConvM3DVToIntAngLenHiRes (m3dV *speed, USHORT *phiC, USHORT *thetaC, USHORT *lenS)
{
   float    phi, theta, len;

   phi   = m3dAngleVector(speed, &m3dVUnitY);
   theta = m3xzAngleVector(speed, &m3dVUnitZ);
   len   = m3dLengthVector(speed);

   *phiC = m3dConvFloatTo16(phi, 0.f, 180.f);
   *thetaC = m3dConvFloatTo16(theta, -180.f, 180.f);
   *lenS = m3dConvFloatTo16(len, 0.f, 500.f);

   return;
}

/***************************************************************
* mpdrConvM3DVTo3Word()
*
* Ranges can be specified on per-level basis
***************************************************************/
void mpdrConvM3DVTo3Word (m3dV *pos, WORD *w1, WORD *w2, WORD *w3)
{
   DWORD    dwX, dwY, dwZ;

   dwY = m3dConvFloatToNBit(pos->y, -250.f, 250.f, 14);
   dwX = m3dConvFloatToNBit(pos->x, -2000.f, 2000.f, 17);
   dwZ = m3dConvFloatToNBit(pos->z, -2000.f, 2000.f, 17);

   *w1 = (WORD)(dwX & 0xFFFF);
   *w2 = (WORD)(dwZ & 0xFFFF);
   *w3 = (WORD)((dwY & 0x3FFF) | ((dwX & 0x10000) >> 2) | ((dwZ & 0x10000) >> 1));
   return;
}

/***************************************************************
* mpdrConv3WordToM3DV()
*
* Ranges can be specified on per-level basis
***************************************************************/
void mpdrConv3WordToM3DV (WORD w1, WORD w2, WORD w3, m3dV *pos)
{
   DWORD    dw;

   dw = w1 | ((w3 & 0x4000) << 2);
   pos->x = m3dConvNBitToFloat(dw, 17, -2000.f, 2000.f);

   dw = w2 | ((w3 & 0x8000) << 1);
   pos->z = m3dConvNBitToFloat(dw, 17, -2000.f, 2000.f);

   dw = (w3 & 0x3FFF);
   pos->y = m3dConvNBitToFloat(dw, 14, -250.f, 250.f);

   return;
}
#endif // SPU

//
// End-of-file M3D_FLOAT2INT.CPP
//
